﻿#include "ShapeBasedMatching.h"
#include <iostream>
#include <QString>
#include <QDir>
#include <QtDebug>

using namespace std;
using namespace cv;

namespace Qly {

ShapeBasedMatching::ShapeBasedMatching()
    : pDetector(nullptr),
    class_id("test")
{
    class_ids.emplace_back(class_id);
}

std::vector<Point2f> ShapeBasedMatching::featurePoints(int template_id)
{
    std::vector<Point2f> ret;
    if(!pDetector) return ret;
    auto templ = pDetector->getTemplates("test", template_id);
    auto feat = templ[0].features;
    for(size_t i = 0; i < feat.size(); i++)
    {
        float x = templ[0].tl_x + feat[i].x - m_width_padding - templateImg.cols / 2.0;
        float y = templ[0].tl_y + feat[i].y - m_height_padding - templateImg.rows / 2.0;
        ret.push_back(cv::Point2f(x, y));
    }
    return ret;
}

std::vector<Point2f> ShapeBasedMatching::featurePoints(const ShapeBasedMatching::MatchResult &mr)
{
    std::vector<Point2f> ret;
    if(!pDetector) return ret;
    auto templ = pDetector->getTemplates("test", mr.template_id);
    auto feat = templ[0].features;
    for(size_t i = 0; i < feat.size(); i++)
    {
        float x = templ[0].tl_x + feat[i].x - m_width_padding - templateImg.cols / 2.0;
        float y = templ[0].tl_y + feat[i].y - m_height_padding - templateImg.rows / 2.0;
        ret.push_back(cv::Point2f(x, y) + mr.rect.center);
    }
    return ret;
}

cv::Mat ShapeBasedMatching::modelImage(int radius, cv::Scalar color, int thickness)
{
    if(infos_have_templ.size() == 0) return cv::Mat();
    int id = infos_have_templ[0].templ_id;
    featureImg = templateImg.clone();
    if(featureImg.empty()) return featureImg;
    auto templ = pDetector->getTemplates(class_id, id);
    auto feat = templ[0].features;
    for(size_t i = 0; i < feat.size(); i++)
    {
        cv::circle(featureImg, { feat[i].x + templ[0].tl_x - m_width_padding, feat[i].y + templ[0].tl_y - m_height_padding }, radius, color, thickness);
    }
    return featureImg;
}

void ShapeBasedMatching::drawFeaturePoints(Mat &img, const std::vector<Point> pts, int radius, Scalar color, int thickness)
{
    for(size_t i = 0; i < pts.size(); i++)
    {
        cv::circle(img, pts[i], radius, color, thickness);
    }
}

void ShapeBasedMatching::drawFeatures(Mat &img, const MatchResult &results,
                                      int radius, Scalar color, int thickness)
{
    drawRotatedRect(img, results.rect, color, thickness);
    std::vector<cv::Point2f> kps = featurePoints(results.template_id);

    for(size_t i = 0; i < kps.size(); i++)
    {
        cv::circle(img,  kps[i] + results.rect.center, radius, color, thickness);
    }
    //drawFeatures(img, kps, radius, color, thickness);
}

std::vector<Point2f> ShapeBasedMatching::featurePoints(const line2Dup::Match &match)
{
    std::vector<Point2f> ret;
    auto templ = pDetector->getTemplates("test", match.template_id);
    auto feat = templ[0].features;
    for(size_t i = 0; i < feat.size(); i++)
    {
        float x = match.x - 32 + feat[i].x;
        float y = match.y - 32 + feat[i].y;
        ret.push_back(cv::Point(x, y));
    }
    return ret;
}

void ShapeBasedMatching::preprocessModelImage(const Mat &modelImage, int width_padding, int height_padding)
{
    if(modelImage.channels() == 4)
    {
        cv::cvtColor(modelImage, templateImg, cv::COLOR_BGRA2BGR);
    }
    else
    {
        templateImg = modelImage.clone();
    }
    m_width_padding = width_padding;
    m_height_padding = height_padding;
    padded_templateImg = cv::Mat(templateImg.rows + 2 * m_height_padding,
                                 templateImg.cols + 2 * m_width_padding,
                                 templateImg.type(),
                                 cv::Scalar::all(0));
    templateImg.copyTo(padded_templateImg(cv::Rect(m_width_padding, m_height_padding, modelImage.cols, modelImage.rows)));

    cv::Mat mask = cv::Mat(modelImage.size(), CV_8UC1, { 255 });
    padded_mask = cv::Mat(padded_templateImg.rows,
                          padded_templateImg.cols,
                          mask.type(),
                          cv::Scalar::all(0));
    mask.copyTo(padded_mask(cv::Rect(m_width_padding, m_height_padding, mask.cols, mask.rows)));
}

RotatedRect ShapeBasedMatching::rotatedRect(const line2Dup::Match &match)
{
    auto templ = pDetector->getTemplates("test", match.template_id);
    // scaling won't affect this, because it has been determined by warpAffine
    // cv::warpAffine(src, dst, rot_mat, src.size()); last param
    float train_img_half_width = templateImg.cols / 2.0f + m_width_padding;
    float train_img_half_height = templateImg.rows / 2.0f + m_height_padding;

    // center x, y of train_img in test img
    float x = match.x - templ[0].tl_x + train_img_half_width - 32;
    float y = match.y - templ[0].tl_y + train_img_half_height - 32;

    //cv::RotatedRect rotatedRectangle({ x, y }, { 2 * r_scaled, 2 * r_scaled }, -infos_have_templ[match.template_id].angle);
    cv::RotatedRect rotatedRectangle(cv::Point2f{ x, y },
                                     cv::Size2f{ float(templateImg.cols), float(templateImg.rows) },
                                     -infos_have_templ[match.template_id].angle);
    return rotatedRectangle;

}

void ShapeBasedMatching::buildFeatureImage(shape_based_matching::shapeInfo_producer::Info &info,
                                           int radius, Scalar color, int thickness)
{
    featureImg = padded_templateImg.clone(); // 获取旋转、缩放之后的图像
    std::vector<line2Dup::Template> templ = pDetector->getTemplates("test", info.templ_id);
    for (size_t i = 0; i < templ[0].features.size(); i++)
    {
        auto feat = templ[0].features[i];
        cv::circle(featureImg, { feat.x + templ[0].tl_x, feat.y + templ[0].tl_y }, radius, color, thickness);
    }
}

Mat ShapeBasedMatching::preprocessTestImage(const Mat &img)
{
    cv::Mat image;
    if(img.channels() == 4)
    {
        cv::cvtColor(img, image, cv::COLOR_BGRA2BGR);
    }
    else
    {
        image = img;
    }
    // make the img having 32*n width & height
    // at least 16*n here for two pyrimads with strides 4 8
    int n = (image.rows + 63) / 64;
    int m = (image.cols + 63) / 64;

    cv::Mat padded_img = cv::Mat(32 + 64 * n,
                                 32 + 64 * m,
                                 image.type(),
                                 cv::Scalar::all(0));
    image.copyTo(padded_img(cv::Rect(32, 32, img.cols, img.rows)));
    return padded_img;
}

void ShapeBasedMatching::refine(const std::vector<line2Dup::Match> &matches, size_t maxCount,
                                std::vector<ShapeBasedMatching::MatchResult> &ret)
{
    ret.clear();
    //将匹配到的最小外接矩形装入vector
    std::vector<cv::RotatedRect> vRects;
    std::vector<float> vScores;
    std::vector<bool> vDeleteIndexs;
    for (size_t i = 0; i < matches.size() ; i++)
    {
        auto match = matches[i];
        cv::RotatedRect rect = rotatedRect(match);
        vRects.push_back(rect);
        vScores.push_back(match.similarity);
        vDeleteIndexs.push_back(false);
    }
    //剔除重复最小外接矩形
    int countDelete = 0;
    cv::RotatedRect rect1, rect2;
    for (size_t i = 0; i < vRects.size() - 1; i++)
    {
        if (vDeleteIndexs[i])
            continue;
        for (size_t j = i + 1; j < vRects.size(); j++)
        {
            if (vDeleteIndexs[j])
                continue;
            rect1 = vRects[i];
            rect2 = vRects[j];
            std::vector<cv::Point2f> vecInterSec;
            int iInterSecType = rotatedRectangleIntersection(rect1, rect2, vecInterSec);
            if (iInterSecType == cv::INTERSECT_NONE)//無交集
                continue;
            else if (iInterSecType == cv::INTERSECT_FULL) //一個矩形包覆另一個
            {
                size_t iDeleteIndex =  (vScores[i] < vScores[j] ? i : j); //匹配分数低的被剔除
                vDeleteIndexs[iDeleteIndex] = true;
                countDelete ++;
            }
            else//交點 > 0
            {
                if (vecInterSec.size() < 3)//一個或兩個交點
                    continue;
                else
                {
                    //求面積與交疊比例
                    double dArea = contourArea(vecInterSec);
                    double dRatio = dArea / rect1.size.area();
                    //若大於最大交疊比例，選分數高的
                    if (dRatio >= 0.5)
                    {
                        size_t iDeleteIndex = (vScores[i] < vScores[j] ? i : j); //匹配分数低的被剔除
                        vDeleteIndexs[iDeleteIndex] = true;
                        countDelete ++;
                    }
                }
            }
        }
    }
    size_t rectCount = vRects.size() - countDelete;
    maxCount = std::min(maxCount, rectCount);

    for(size_t i = 0; i < vRects.size() && maxCount != 0; i++)
    {
        if( !vDeleteIndexs[i] )
        {
            MatchResult mr;
            mr.rect = vRects[i];
            mr.score = vScores[i];
            mr.template_id = matches[i].template_id;
            //mr.keyPoints = featurePoints(matches[i]);
            ret.push_back(mr);
            maxCount--;
        }
    }
}

}



